use super::pair::{pair,Asf64};
use super::time::{UTime,SysTime,UClockData,Clocker,UClock};
use super::clk::{ClockTracer};
use std::thread;
use std::time::Duration;

#[derive(Debug)]
pub struct RLS<const A:f64> {
    ex:f64,
    ey:f64,
    dx:f64,
    dy:f64,
    exy:f64,
    e:f64,
    ee:f64,
    a:f64,
    active:bool,
}

impl<const A:f64> RLS<A> {
    pub fn new(d:&UClockData) -> Self {
        Self { 
            ex : d.cpu_start.0.asf64(),
            ey : d.self_start.0.asf64(),
            e : 0f64,
            ee : 0f64,
            dx : 0f64,
            exy: 0f64,
            dy: 0f64,
            a : d.rate_nano,
            active:false,
        }
    }
    fn merge(x0:f64,x1:f64) -> f64 {A*(x1-x0)+x0}
    pub fn add(&mut self,p:pair<UTime,SysTime>) {
        let x = p.1.asf64();
        let y = p.0.asf64();
        let x1 = x-self.ex;
        let y1 = y-self.ey;
        let e = y1-self.a*x1;
        self.ee = Self::merge(self.ee,e);
    }
    fn update0(&mut self,p:pair<UTime,SysTime>) -> bool {
        let dx = p.1.asf64() - self.ex;
        let dy = p.0.asf64() - self.ey;
        let x1 = dy/self.a-dx;
        let y1 = dx*self.a-dy;
        self.dx = Self::merge(self.dx,x1*x1);
        self.exy = Self::merge(self.exy,y1*x1);
        self.dy = Self::merge(self.dy,y1*y1);
        self.a = Self::merge(self.a,self.exy/self.dx);
        let nx1 = dy/self.a;
        let ny1 = dx*self.a;
        self.ex += Self::merge(nx1,dx);
        self.ey += Self::merge(ny1,dy);
        self.active = true;
        println!("x:{} x1:{},y:{} y1:{},{:?}",dx,x1,dy,y1,self);
        self.active 
    }
}


impl<const A:f64> ClockTracer for RLS<A> {
    fn update(&mut self,p:pair<UTime,SysTime>) -> bool {
        let x = p.1.asf64();
        let y = p.0.asf64();
        let ex = Self::merge(self.ex,x);
        let ey = Self::merge(self.ey,y);
        let x1 = x-ex;
        let y1 = y-ey;
        self.dx = Self::merge(self.dx,x1*x1);
        self.exy = Self::merge(self.exy,y1*x1);
        self.dy = Self::merge(self.dy,y1*y1);
        self.a = self.exy/self.dx;
        self.ex = ex;
        self.ey = ey;
        self.e = y1-self.a*x1;
        self.ee = Self::merge(self.ee,self.e);
        self.active = true;
        println!("x:{} x1:{},y:{} y1:{},{:?}",x,x1,y,y1,self);
        self.active 
    }

    fn error(&self) -> Option<f64> {
        if !self.active {
            None
        } else {
            Some(self.dy)
        }
    }
    fn getdata(&self) -> UClockData {
        UClockData { 
            rate_nano:self.a, 
            cpu_start:SysTime(self.ex as i64), 
            self_start:UTime((self.ey+self.ee) as i64),
            quality:self.dy,
        }
    }
}

pub struct RLSex {
    pub l0 : RLS<0.03>,
}
impl RLSex {
    pub fn new(d:&UClockData) -> Self {
        Self { l0:RLS::<0.03>::new(d) }
    }
    pub fn one_ls<C:Clocker+Default>(&mut self) -> Option<UClock<C>> {
        let mut tempc = C::default();
        for _ in 0..10 {
            let p = pair(tempc.now(),SysTime::now());
            self.l0.add(p);
            thread::sleep(Duration::from_millis(100));
        }
        let p = pair(tempc.now(),SysTime::now());
        if self.l0.update(p) {
            Some(UClock{ data: self.l0.getdata(), clock:C::default()})
        } else {
            None
        }
    }
}